
#include "can.h"

#include "oslib.h"

#include "dji2_0.h"

#include "tripod.h"
#include "shooter.h"

/*-define--------------------------------------------*/
#define CTRL_INTV    20
#define LOAD_POS     -57000

#define TRIPOD_EVENT_RUN        (1UL<<0)
#define TRIPOD_EVENT_POS_READY  (1UL<<1)

/*-typedefs------------------------------------------*/
typedef struct _tripod_t {
    CAN_HandleTypeDef *hcan;
    int dji_board_id;
    int dji_motor_id;
    uint8_t init;
    uint8_t run;
    uint8_t arrive;
    TripodMode mode;
    PID_s pid;
    osThreadId_t task;
    osEventFlagsId_t events;
    BoxType boxes[1];
} tripod_t;

/*-static variables----------------------------------*/
tripod_t __tripod1, *tripod1=&__tripod1;

const osThreadAttr_t tripod_task_attrs = {
    .name = "TripodTask",
    .priority = osPriorityLow,
    .stack_size = 192 * 4,
};

/*-static prototypes---------------------------------*/
static void tripod_task(void *argument)
{
    tripod_handle_t tpd = (tripod_handle_t)argument;
    uint32_t evt;
    /* 初始化 */
    for (;;) {
        // 发命令找零
        dji_Homing(tpd->hcan, tpd->dji_board_id, tpd->dji_motor_id, 50, 5000);
        // 等待找零完成
        evt = osEventFlagsWait(tpd->events, TRIPOD_EVENT_POS_READY, osFlagsWaitAny, 2000);
        if (evt < osStatusReserved && (evt & TRIPOD_EVENT_POS_READY) == TRIPOD_EVENT_POS_READY) {
            break;
        }
    }
    tpd->init = 1;

    for (;;) {
        tpd->arrive = 0;
        // 旋转到上箭位
        for (;;) {
            // 发命令跑位置环
            dji_NoInitPosCfg(tpd->hcan, tpd->dji_board_id, tpd->dji_motor_id, 50);
            dji_PosCtrl(tpd->hcan, tpd->dji_board_id, tpd->dji_motor_id, LOAD_POS);
            // 等待位置环到位 // FIXME: can报文可能会丢, 这里可以改成循环发送
            evt = osEventFlagsWait(tpd->events, TRIPOD_EVENT_POS_READY, osFlagsWaitAny, CTRL_INTV);
            if (evt < osStatusReserved && (evt & TRIPOD_EVENT_POS_READY) == TRIPOD_EVENT_POS_READY) {
                tpd->arrive = 1;
                break;
            }
        }
        tpd->mode = MANUAL;
        

    }
}


/*-static functions----------------------------------*/
static 